#include "Munjiza_nbs.h"

//#define TESTES
Munjiza_nbs::Munjiza_nbs()
{
    Spheres = NULL;
    MinR[0] = MinR[1] = MaxR[0] = MaxR[1] = 0.0;
}

Munjiza_nbs::~Munjiza_nbs()
{
    for (int i=0; i!=Cols; i++)
    {
        colunasLinhaAtual[i]->esferas.clear();
        colunasLinhaPosterior[i]->esferas.clear();
        #ifdef TESTES
        assert (colunasLinhaAtual[i]->esferas.empty());
        assert (colunasLinhaPosterior[i]->esferas.empty());
        #endif
        delete colunasLinhaAtual[i];
        delete colunasLinhaPosterior[i];
    }
    for (int i=0; i!=Rows; i++)
    {
        linhas[i]->esferas.clear();
        #ifdef TESTES
        assert (linhas[i]->esferas.empty());
        #endif
        delete linhas[i];
    }
}

void Munjiza_nbs::iniciaMatriz( REAL max_rad, REAL min_r[2], REAL max_r[2] )
{

        MinR[0] = min_r[0];
        MinR[1] = min_r[1];
        MaxR[0] = max_r[0];
        MaxR[1] = max_r[1];

        tamCelula = max_rad*2.0;
        Rows = (int32_t) ((MaxR[1] - MinR[1]) / tamCelula);
        Cols = (int32_t) ((MaxR[0] - MinR[0]) / tamCelula);

        int qtdEsferas=0;
        for (SPHERE* s = Spheres->Begin; s != NULL; s = s->Next)
        {
            qtdEsferas++;
        }
        printf ("\tMunjiza-NBS -> Vetor-Coluna:%d Vetor-Linha:%d\n\t",Cols,Rows);
        ImprimeMem("Munjiza-NBS",(((2*Cols + Rows)*sizeof(elementoColuna)) + 2*qtdEsferas*sizeof(SPHERE*)));
        alocaElementosNaLinha();
        alocaElementosNaColuna();
}

void Munjiza_nbs::alocaElementosNaColuna ()
{
    for (int i=0; i!=Cols; i++)
    {
        elementoColuna *elem = new elementoColuna();
        elementoColuna *elem2 = new elementoColuna();
        colunasLinhaAtual.push_back(elem);
        colunasLinhaPosterior.push_back(elem2);
    }
}

void Munjiza_nbs::alocaElementosNaLinha ()
{
    for (int i=0; i!=Rows; i++)
    {
        elementoLinha *elem = new elementoLinha();
        linhas.push_back(elem);
    }
}

void Munjiza_nbs::calcular()
{
    #pragma omp single
    {
         #ifdef TESTES
         int iteracoes=0;
         #endif

        //Etapa 1: Popular o vetor-linha
        for (SPHERE* s = Spheres->Begin; s != NULL; s = s->Next)
        {
            int32_t row = Max( Min( (int32_t)( (s->R[1] - MinR[1]) / tamCelula ), Rows-1 ), 1 );
            linhas[row]->novo=true;
            linhas[row]->esferas.push_back(s);
        }
        #ifdef TESTES
        //Teste para saber se realmente o elemento foi inserido
        assert (!linhas.empty());
        assert (linhas.size()<=Rows);
        #endif

        //Etapa 2

        for (SPHERE* s = Spheres->Begin; s != NULL; s = s->Next)
        {
            int row = Max( Min( (int32_t)( (s->R[1] - MinR[1]) / tamCelula ), Rows-1 ), 0);

            if (linhas[row]->novo)
            {
                //2.2  Se a posição for “nova”, esta será marcada como “observada”.
                linhas[row]->novo=false;
                //2.2.1 Percorro toda a lista de elementos da linha atual
                for (vector<SPHERE*>::iterator it=linhas[row]->esferas.begin(); it!=linhas[row]->esferas.end(); ++it)
                {
                    //Pego o elemento atual
                    SPHERE* esferaAtual = *it;

                    //Calculo a posição do elemento no vetor-coluna
                    int32_t col = Max( Min( (int32_t)( (esferaAtual->R[0] - MinR[0]) / tamCelula ), Cols-1 ), 0 );

                    //Inicializo as posições populadas como novas
                    colunasLinhaAtual[col]->novo=true;

                    #ifdef TESTES
                    //Teste para saber se realmente o elemento foi inserido
                    int sizen = colunasLinhaAtual[col]->esferas.size();
                    #endif

                    //coloco cada elemento na sua respectiva posição no “vetor-coluna”
                    colunasLinhaAtual[col]->esferas.push_back(esferaAtual);

                    #ifdef TESTES
                    assert(sizen+1==colunasLinhaAtual[col]->esferas.size());
                    #endif
                }
                #ifdef TESTES
                if (!linhas[row]->esferas.empty())
                {
                    assert (!colunasLinhaAtual.empty());
                    assert (colunasLinhaAtual.size()<=Cols);
                }
                #endif

                //2.2.2 Idem ao anterior para a linha posterior, com cuidado para não passar da última linha

                if (row+1<Rows)
                {
                    for (vector<SPHERE*>::iterator it=linhas[row+1]->esferas.begin(); it!=linhas[row+1]->esferas.end(); ++it)
                    {
                        SPHERE* esferaAtual = *it;
                        int32_t col = Max( Min( (int32_t)( (esferaAtual->R[0] - MinR[0]) / tamCelula ), Cols-1 ), 0 );
                        colunasLinhaPosterior[col]->novo=true;
                        colunasLinhaPosterior[col]->esferas.push_back(esferaAtual);
                    }
                    #ifdef TESTES
                    if (!linhas[row+1]->esferas.empty())
                    {
                        assert (!colunasLinhaPosterior.empty());
                        assert (colunasLinhaPosterior.size()<=Cols);
                    }
                    #endif
                }
                //printf ("\n%d seg",row);

                //2.2.3 Percorro toda a lista de elementos da linha atual
                for (int posEsfLinha=0; posEsfLinha!=linhas[row]->esferas.size(); ++posEsfLinha)
                {
                    //Pego o elemento atual
                    SPHERE* esferaAtual = linhas[row]->esferas[posEsfLinha];
                    //Calculo a posição do elemento no vetor-coluna
                    int32_t col = Max( Min( (int32_t)( (esferaAtual->R[0] - MinR[0]) / tamCelula ), Cols-1 ), 0 );

                    // 2.2.3.1  Se esta posição estiver marcada como “nova”
                    if (colunasLinhaAtual[col]->novo)
                    {
                         //2.2.3.1.1  Marque-a como “observada”
                         colunasLinhaAtual[col]->novo=false;

                         //2.2.3.1.2  Faça a detecção de contato com (x,y),(x,y-1),(x+1,y-1), (x+1,y), (x+1,y+1)
                         for (int i=0, tamanhoVetor=colunasLinhaAtual[col]->esferas.size(); i!=tamanhoVetor; i++)
                         {

                             esferaAtual = colunasLinhaAtual[col]->esferas[i];

                             //Comparo os elementos da própria célula (x,y)
                             for (int j=i+1; j!=tamanhoVetor; j++)
                             {
                                 SPHERE* esferaTmp = colunasLinhaAtual[col]->esferas[j];
                                 #ifdef TESTES
                                 iteracoes++;
                                 #endif
                                 Forca->ForcaResSS(esferaAtual,esferaTmp);
                             }
                             //Testo  com os elementos da coluna anterior, se existir
                             if (col>0)
                             {
                                 //Com a coluna anterior testo com os elementos da linha atual (x,y-1)
                                 for (int j=0,tamanhoVetor=colunasLinhaAtual[col-1]->esferas.size(); j!=tamanhoVetor; j++)
                                 {
                                     SPHERE* esferaTmp = colunasLinhaAtual[col-1]->esferas[j];
                                     #ifdef TESTES
                                     iteracoes++;
                                     #endif
                                     Forca->ForcaResSS(esferaAtual,esferaTmp);
                                 }

                                 //Com a coluna anterior testo com os elementos da linha posterior, se existirem (x+1,y-1)
                                 if (!colunasLinhaPosterior.empty())
                                 {
                                     for (int j=0,tamanhoVetor=colunasLinhaPosterior[col-1]->esferas.size(); j!=tamanhoVetor; j++)
                                     {
                                         SPHERE* esferaTmp = colunasLinhaPosterior[col-1]->esferas[j];
                                         #ifdef TESTES
                                         iteracoes++;
                                         #endif
                                         Forca->ForcaResSS(esferaAtual,esferaTmp);
                                     }
                                 }
                             }
                             //Com a coluna atual testo com os elementos da linha posterior, se existirem

                             if (!colunasLinhaPosterior.empty())
                             {
                                 // (x+1,y)
                                 for (int j=0,tamanhoVetor=colunasLinhaPosterior[col]->esferas.size(); j!=tamanhoVetor; j++)
                                 {
                                     SPHERE* esferaTmp = colunasLinhaPosterior[col]->esferas[j];
                                     #ifdef TESTES
                                     iteracoes++;
                                     #endif
                                     Forca->ForcaResSS(esferaAtual,esferaTmp);
                                 }

                                 // (x+1,y+1)
                                 if (col+1<colunasLinhaPosterior.size())
                                 {
                                     for (int j=0,tamanhoVetor=colunasLinhaPosterior[col+1]->esferas.size(); j!=tamanhoVetor; j++)
                                     {
                                         SPHERE* esferaTmp = colunasLinhaPosterior[col+1]->esferas[j];
                                         #ifdef TESTES
                                         iteracoes++;
                                         #endif
                                         Forca->ForcaResSS(esferaAtual,esferaTmp);
                                     }
                                 }
                             }
                         }
                    }

                }
                //  2.2.4  .Percorra todos os elementos da linha atual e remova os elementos do vetor-coluna.
                //Remova a cabeça da lista.
                //  2.2.5  .Percorra todos os elementos da linha atual e remova os elementos do vetor-coluna.
                //Remova a cabeça da lista.
                for (int i=0; i!=Cols; i++)
                {
                    colunasLinhaAtual[i]->esferas.clear();
                    colunasLinhaPosterior[i]->esferas.clear();
                    #ifdef TESTES

                    assert (colunasLinhaAtual[i]->esferas.empty());
                    assert (colunasLinhaPosterior[i]->esferas.empty());
                    #endif
                }
            }
        }

        #ifdef TESTES
        printf ("\n%d\n",iteracoes);
        #endif
        // 3  Percorra todos os elementos e remova as linhas do “vetor-linha”.
        for (int i=0; i!=Rows; i++)
        {
            linhas[i]->esferas.clear();
        }
    }
    #pragma omp barrier
}
